﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnitySteer.Behaviors;





namespace UnitySteer.Behaviors
{


    public class ObstacleAvoidance : UnitySteer.Behaviors.Steering
        //: UnitySteer.Behaviors.Steering
    {
        private struct Line
        {
            public Vector2 direction;
            public Vector2 point;
        }




        [SerializeField]bool predictObstacle;
        [SerializeField]float timeHorizon_ = 0.2f;
        [SerializeField]float timeHorizonObst_ = 0.2f;
        static float RVO_EPSILON = 0.001f;
        IList<Vehicle> agentNeighbors_ = new List<Vehicle>();
        IList<Line> orcaLines_ = new List<Line>();




        protected override void Start()
        {
            
            base.Start();
            Vehicle.Radar.OnDetected += HandleDetection;
        }

        void HandleDetection(Radar radar)
        {
            agentNeighbors_.Clear();
            // I'd prefer an iterator, but trying to cut down on allocations
            for (var i = 0; i < radar.Vehicles.Count; i++)
            {
                var other = radar.Vehicles[i];
                agentNeighbors_.Add(other);
            }
        }

        protected override Vector3 CalculateForce()
        {
            return computeNewVelocity();
            
        }

        static float det(Vector2 vector1, Vector2 vector2)
        {
            return vector1.x * vector2.y - vector1.y * vector2.x;
        }



        public override bool IsPostProcess
        {
            get { return true; }
        }




        Vector3 computeNewVelocity()
        {

            UnityEngine.Profiling.Profiler.BeginSample("ObstacleAvoidance");
            orcaLines_.Clear();
            int numObstLines = orcaLines_.Count;
            float invTimeHorizonObst = 1.0f / timeHorizonObst_;
            float invTimeHorizon = 1.0f / timeHorizon_;
            float timeStep = Vehicle.DeltaTime;
            float MaxSpeed = Vehicle.MaxSpeed;
            Vector2 Velocity = new Vector2(Vehicle.Velocity.x, Vehicle.Velocity.z);
            Vector2 desireVelocity = new Vector2(Vehicle.DesiredVelocity.x, Vehicle.DesiredVelocity.z);
            //Vector2 desireVelocity = new Vector2(0, -1);

            for (int i = 0; i < agentNeighbors_.Count; ++i)
            {
                Vehicle other = agentNeighbors_[i];


                Vector2 relativePosition = new Vector2((other.Position - Vehicle.Position).x, (other.Position - Vehicle.Position).z);
                Vector2 relativeVelocity = new Vector2((Vehicle.Velocity- other.Velocity).x, (Vehicle.Velocity- other.Velocity).z);

                float distSq = relativePosition.magnitude * relativePosition.magnitude;
                float combinedRadius = Vehicle.Radius + other.Radius;
                float combinedRadiusSq = combinedRadius*combinedRadius;

                Line line;
                Vector2 u;

                if (distSq > combinedRadiusSq && predictObstacle)
                {

                    /* No collision. */
                    Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;

                    /* Vector from cutoff center to relative velocity. */
                    float wLengthSq = w.magnitude*w.magnitude;
                    float dotProduct1 = Vector2.Dot(w ,relativePosition);

                    if (dotProduct1 < 0.0f && dotProduct1* dotProduct1 > combinedRadiusSq * wLengthSq)
                    {
                        /* Project on cut-off circle. */
                        float wLength = Mathf.Sqrt(wLengthSq);
                        Vector2 unitW = w / wLength;

                        line.direction = new Vector2(unitW.y, -unitW.x);
                        u = (combinedRadius * invTimeHorizon - wLength) * unitW;
                    }
                    else
                    {
                        /* Project on legs. */
                        float leg = Mathf.Sqrt(distSq - combinedRadiusSq);

                        if (relativePosition.x * w.y - relativePosition.y * w.x > 0.0f)
                        {
                            /* Project on left leg. */
                            line.direction = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
                        }
                        else
                        {
                            /* Project on right leg. */
                            line.direction = -new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq;
                        }

                        float dotProduct2 = Vector2.Dot(relativeVelocity ,line.direction);
                        u =  dotProduct2 * line.direction - relativeVelocity;
                    }
                    
                }
                else
                {
                    /* Collision. Project on cut-off circle of time timeStep. */
                    float invTimeStep = 1.0f / timeStep;

                    /* Vector from cutoff center to relative velocity. */
                    Vector2 w = relativeVelocity - invTimeStep * relativePosition;

                    float wLength = w.magnitude;
                    Vector2 unitW = w / wLength;

                    line.direction = new Vector2(unitW.y, -unitW.x);
                    u = (combinedRadius * invTimeStep - wLength) * unitW;
                }

                line.point = Velocity + 0.5f * u;
                orcaLines_.Add(line);
            }

            Vector2 newVelocity = new Vector2();
            Vector3 forceDir = new Vector3();

            int lineFail = linearProgram2(orcaLines_, MaxSpeed, desireVelocity, false, ref newVelocity);
            if (lineFail < orcaLines_.Count)
            {
                linearProgram3(orcaLines_, numObstLines, lineFail, MaxSpeed, ref newVelocity);
                //newVelocity = desireVelocity;
            }


            forceDir = new Vector3((newVelocity).x, 0, (newVelocity).y).normalized;

            UnityEngine.Profiling.Profiler.EndSample();
            if (Vector2.Angle(newVelocity, desireVelocity) < 0.1f)
            {
                return Vector3.zero;
            }
            return forceDir;
        }




        private bool linearProgram1(IList<Line> lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        {
            float dotProduct = Vector2.Dot(lines[lineNo].point , lines[lineNo].direction);
            float discriminant = dotProduct* dotProduct + radius* radius - lines[lineNo].point.magnitude* lines[lineNo].point.magnitude;

            if (discriminant < 0.0f)
            {
                /* Max speed circle fully invalidates line lineNo. */
                return false;
            }

            float sqrtDiscriminant = Mathf.Sqrt(discriminant);
            float tLeft = -dotProduct - sqrtDiscriminant;
            float tRight = -dotProduct + sqrtDiscriminant;

            for (int i = 0; i < lineNo; ++i)
            {

                float denominator = det(lines[lineNo].direction, lines[i].direction);
                float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);

                if (Mathf.Abs(denominator) <= RVO_EPSILON)
                {
                    /* Lines lineNo and i are (almost) parallel. */
                    if (numerator < 0.0f)
                    {
                        return false;
                    }

                    continue;
                }

                float t = numerator / denominator;

                if (denominator >= 0.0f)
                {
                    /* Line i bounds line lineNo on the right. */
                    tRight = Math.Min(tRight, t);
                }
                else
                {
                    /* Line i bounds line lineNo on the left. */
                    tLeft = Math.Max(tLeft, t);
                }

                if (tLeft > tRight)
                {
                    return false;
                }
            }

            if (directionOpt)
            {
                /* Optimize direction. */
                if (Vector2.Dot(optVelocity  ,lines[lineNo].direction) > 0.0f)
                {
                    /* Take right extreme. */
                    result = lines[lineNo].point + tRight * lines[lineNo].direction;
                }
                else
                {
                    /* Take left extreme. */
                    result = lines[lineNo].point + tLeft * lines[lineNo].direction;
                }
            }
            else
            {
                /* Optimize closest point. */
                float t = Vector2.Dot(lines[lineNo].direction , (optVelocity - lines[lineNo].point));

                if (t < tLeft)
                {
                    result = lines[lineNo].point + tLeft * lines[lineNo].direction;
                }
                else if (t > tRight)
                {
                    result = lines[lineNo].point + tRight * lines[lineNo].direction;
                }
                else
                {
                    result = lines[lineNo].point + t * lines[lineNo].direction;
                }
            }

            return true;
        }


        private int linearProgram2(IList<Line> lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result)
        {
            if (directionOpt)
            {
                /*
                 * Optimize direction. Note that the optimization velocity is of
                 * unit length in this case.
                 */
                result = optVelocity * radius;
            }
            else if (optVelocity.magnitude*optVelocity.magnitude > radius*radius )
            {
                /* Optimize closest point and outside circle. */
                result = optVelocity.normalized * radius;
            }
            else
            {
                /* Optimize closest point and inside circle. */
                result = optVelocity;
            }

            for (int i = 0; i < lines.Count; ++i)
            {
                if (det(lines[i].direction, lines[i].point - result) > 0.0f)
                {
                    /* Result does not satisfy constraint i. Compute new optimal result. */
                    Vector2 tempResult = result;
                    if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result))
                    {
                        result = tempResult;

                        return i;
                    }
                }
            }

            return lines.Count;
        }


        private void linearProgram3(IList<Line> lines, int numObstLines, int beginLine, float radius, ref Vector2 result)
        {
            float distance = 0.0f;

            for (int i = beginLine; i < lines.Count; ++i)
            {
                if (det(lines[i].direction, lines[i].point - result) > distance)
                {
                    /* Result does not satisfy constraint of line i. */
                    IList<Line> projLines = new List<Line>();
                    for (int ii = 0; ii < numObstLines; ++ii)
                    {
                        projLines.Add(lines[ii]);
                    }

                    for (int j = numObstLines; j < i; ++j)
                    {
                        Line line;

                        float determinant = det(lines[i].direction, lines[j].direction);

                        if (Mathf.Abs(determinant) <= RVO_EPSILON)
                        {
                            /* Line i and line j are parallel. */
                            if (Vector2.Dot(lines[i].direction ,lines[j].direction) > 0.0f)
                            {
                                /* Line i and line j point in the same direction. */
                                continue;
                            }
                            else
                            {
                                /* Line i and line j point in opposite direction. */
                                line.point = 0.5f * (lines[i].point + lines[j].point);
                            }
                        }
                        else
                        {
                            line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
                        }

                        line.direction = (lines[j].direction - lines[i].direction).normalized;
                        projLines.Add(line);
                    }

                    Vector2 tempResult = result;
                    if (linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y, lines[i].direction.x), true, ref result) < projLines.Count)
                    {
                        /*
                         * This should in principle not happen. The result is by
                         * definition already in the feasible region of this
                         * linear program. If it fails, it is due to small
                         * floating point error, and the current result is kept.
                         */
                        result = tempResult;
                    }

                    distance = det(lines[i].direction, lines[i].point - result);
                }
            }
        }

    }

}
